/**************************************************************
 * Ben Stephenson       07025656
 * Mark Renney          07010390
 * Laurie Potts         07009314
 * Michael Peeke-Vout   07014312
 *
 * CM0034 Industrial Case Project
 * Robot Movements
 * robot_movements.h
 * 28/02/2011
 * Updated - 16/05/11 
 **************************************************************/

#ifndef __ROBOT_MOVEMENTS_H
#define __ROBOT_MOVEMENTS_H

#include <stdint.h>
#include <stdbool.h>
#include <robot.h>
#include <lcd.h>
#include <ucos_ii.h>

#define DEFAULT_MIDDLE 68750
#define HAND_SAFE 95000

#define WAIST_DEFAULT 66000
#define ELBOW_DEFAULT 95000
#define WRIST_DEFAULT 83750
#define HAND_DEFAULT 60250

#define INPUT_TO_CONVEYOR 49000
#define CONVEYOR_TO_INPUT 95000
#define LIFT_HEIGHT_INPUT 80000
#define ELBOW_DROP_INPUT 95000
#define WRIST_DROP_INPUT 95000

#define OUTPUT_TO_CONVEYOR 47500
#define CONVEYOR_TO_OUTPUT 91000
#define ELBOW_DROP_OUTPUT 87000
#define ELBOW_DROP_OUPUT 90000
#define WRIST_DROP_OUTPUT 80000
#define ELBOW_DROP_SEVERE_OUTPUT 95000
#define ELBOW_DEFAULT_OUTPUT 72500
#define WRIST_DEFAULT_OUTPUT 72500

#define IDLE 0
#define MOVING 1

#define IN_PAD 0
#define CON_PAD 1

#define OUT_PAD 0
#define CON_OUT_PAD 1

void MoveRobot (robotJoint_t, uint32_t);
void Default_Positions (void);
void Robot_Collect_Block_Part1 (void);
void Robot_Collect_Block_Part2 (void);
void Robot_Remove_Block_Part1 (void);
void Robot_Remove_Block_Part2 (void);

uint8_t getRobotState(void);
void setRobotState(uint8_t);

bool getPadState(uint8_t);
void setPadState(uint8_t, bool);
void initRobotVars(void);

#endif